#include "pfsm_drag.h"
#include "axisgroup_robot.h"
#include "axis/axis.h"
#include "log/zuclog.h"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"

using namespace axisgroup;

#define MOTDRAG_INFO(fmt, ...) zuclog_info("MOTDRAG", fmt, ##__VA_ARGS__)
#define MOTDRAG_ERR(fmt, ...) zuclog_error("MOTDRAG", fmt, ##__VA_ARGS__)
#define MOTDRAG_DEBUG(fmt, ...) zuclog_debug("MOTDRAG", fmt, ##__VA_ARGS__)

PFsmDrag::PFsmDrag(Robot* grp) : PFsmState(grp) {}

int PFsmDrag::update_plan()
{
    //检查TCP速度限制, 超速退出
    if (check_tcp_vel_limit(grp_->get_moving_state().fdb.linear_fdb_vel))
    {
        return -1;
        //TODO:切换到ErrorStop状态，做cat2stop,并退出拖拽模式
    }

    // 拖拽时，指令位置跟随反馈位置
    grp_->cmd_rst_.jpos = grp_->get_moving_state().fdb.fdb_pos;
    grp_->kine_->fkine(grp_->cmd_rst_.jpos,
                       grp_->cmd_rst_.pose,
                       &(RobotToolMgr::get_rob_tool(grp_->tool_id_).pose()),
                       &(UsrFrameMgr::get_usr_frame(grp_->wobj_id_).world(grp_->base_frame_id_)));
    return 0;
}

int PFsmDrag::enter()
{
    exit_cmd_ = 0;
    return 0;
}

void axisgroup::PFsmDrag::exit() { exit_cmd_ = true; }

bool axisgroup::PFsmDrag::is_stopping() { return false; }

bool axisgroup::PFsmDrag::is_plan_over() { return true; }

// 拖拽模式，始终处于非inpos状态
bool axisgroup::PFsmDrag::is_fdb_inpos()
{
    if (exit_cmd_)
        return true;
    return false;
}

int PFsmDrag::check_tcp_vel_limit(double cur_tcp_vel) { return 0; }